#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
import time
import numpy as np
import math

#多线程参考：https://www.cnblogs.com/0-lingdu/p/10068301.html
threadingExitFlag = 0


class SnakeRobot:     
    a = 35  #蛇的运动公式里面的参数（幅度）
    b = 65  #蛇的运动公式里面的参数（跨度）
    g = 0   #蛇的运动公式里面的参数（用来控制左右方向）
    w = 20  #蛇的运动公式里面的参数（角度变化频率）
    _lasttime = -1

    period = 5 #角度刷新周期（每隔这么多时间刷新一次角度）
    motors_quanity = 5
    #关节数量，这里默认6，有需要可以灵活设置
    isGoingForward = 0
    #是否向前运动，true为向前，false向后
    stopMoving_signal = 0  #是否停止运动标志，true为停止
      #蛇运动的累计时间，在程序中每隔一个period时间就增加0.1
    t0 = 0 
    t1 = 0 #蛇的运动公式里面的参数（用在前后方向切换时，作为时间补偿，以保证切换顺畅）
    m = 0
    n = 0
    stime = 0
    #前后方向切换的标志位
    delta = []
    xita = []
    delta_back = []
    xita_back = []


    #ID 列表 T T T ......
    _ids = []
  
    #初始化机器人：使用 BwRobotLib 对象
    def __init__(self,mclib):
        self.lib = mclib

    def bindIds(self,ids):
        size = len(ids)
        self.motors_quanity = size
        self._ids = [0] * size
        self.xita = [0] * size
        self.xita_back = [0] * size
        self.delta = [0] * size
        self.delta_back = [0] * size
        for i in range(size):
            self._ids[i] = ids[i]

#name : angle_i_forward
#decription : 计算前进时的单个关节角度
#arguments : a 幅度；b 跨度；c 用来控制左右方向（也就是上面的g）；t 蛇运动的累计时间（time）
#                    i 表示第i个关节；w 角度变化频率
#return :角度
#
    def angle_i_forward(self,a, b, c, t, i, w):
      t0 = self.t0
      t1 = self.t1

      cnt = 0.0
      cnt = w * t + (i + 0.5) * b + 2 * w * t0 - 2 * w * t1
      cnt = a * math.sin(cnt * math.pi / 180) + c
      return(cnt)

#name : angle_i_back
#decription : 计算后退时的单个关节角度
#arguments : a 幅度；b 跨度；c 用来控制左右方向（也就是上面的g）；t 蛇运动的累计时间（time）
#                    i 表示第i个关节；w 角度变化频率
#return :角度
#
    def angle_i_back(self,a, b, c, t, i, w):
      t0 = self.t0
      t1 = self.t1

      cnt = 0.0
      cnt = -w * t + (i + 0.5) * b + 2 * w * t0 - 2 * w * t1
      cnt = a * math.sin(cnt * math.pi / 180) + c
      return(cnt)

    def mstime(self):
      t = time.time()
      ms = (int(round(t * 1000)))
      return ms

#name : snake_move
#decription : 每运行一次就刷新一遍角度，并设置一遍所有关节的角度
#arguments :无
#return :void
#
    def snake_move(self):
        a = self.a
        b = self.b
        g = self.g
        w = self.w
        for number in range(self.motors_quanity):
            self.delta[number] = self.angle_i_forward(a, b, g, self.stime, number, w)
            self.xita[number] = self.delta[number]
            self.delta_back[number] = int(self.angle_i_back(a, b, g, self.stime, number, w))
            self.xita_back[number] = int(self.delta_back[number])
        if(self.isGoingForward):
            self.m = 1
            if(self.m * self.n > 0):
                #这里是前后切换了，要把后退时最后那个状态的所有角度保留下来
                self.n = 0
                self.t1 = self.stime + self.t1
                for number in range(self.motors_quanity):
                    self.delta[number] = int(self.angle_i_forward(a, b, g, self.stime, number, w))
                    self.xita[number] =  int(self.delta[number])
                    #print(self.delta[number])
                    #print(self.xita[number])
            self.lib.SetMotorPositionSync(self._ids, self.xita)
        else: #后退
            self.n = 1
            if(self.m * self.n > 0):
                self.m = 0
                self.t0 = self.stime + self.t0
                for number in range(self.motors_quanity):
                    self.delta_back[number] =  int(self.angle_i_back(a, b, g, self.stime, number, w))
                    self.xita_back[number] =  int(self.delta_back[number])
                    #print(self.delta[number])
                    #print(self.xita[number])
            self.lib.SetMotorPositionSync(self._ids, self.xita_back)
        pass 


#name : move
#decription : 用于跑循环，在while循环里执行snake_move（）函数
#arguments :无
#return :void
    def move(self):
        lasttime = 0
        nowtime = 0
        self.stime = 0
        while(1):
            nowtime = self.mstime()
            if(nowtime - lasttime > self.period):
                if(self.stopMoving_signal == 0):
                   self.stime = self.stime + 0.1 #至于为什么每隔一个period时间是加0.1，这个关系到一个周期内角度刷新的密集程度
                   self.snake_move()
                   lasttime = self.mstime()
            time.sleep(0.001)


    def run(self):
        self.stopMoving_signal = 0
        self.move()
        pass

    def goForward(self):
        print("向前走")
        self.stopMoving_signal = 0
        self.isGoingForward = 1
        self.g = 0
        pass


    def goBack(self):
        print("向后走")
        self.stopMoving_signal = 0
        self.isGoingForward = 0
        self.g = 0
        pass

    def stop(self):
        print("停止")
        self.stopMoving_signal = 1
        pass

    def turnLeft(self):
        print("左转")
        self.stopMoving_signal = 0
        self.g = -5
        pass

    def turnRight(self):
        print("右转")
        self.stopMoving_signal = 0
        self.g = 5
        pass

    def setVelocity(self,percent):
	    #name : setVelocity
	    #decription : 速度值改变时要执行的函数，它是通过改变公式里的参数period
	    #arguments : speed 速度，speed越大，实际运动速度越慢（范围取0-100不等，如果范围感觉过大，可以缩小可调范围）
	    #return :void
	    #
        self.period = 40 - (int)(40 * percent / 100.0)
        pass

    def setSwingRange(self,percent):
	     #name : set_a：修改振幅
	     #decription : a值改变时要执行的函数，它是通过改变公式里的参数a
	     #arguments : the_a 运动波形幅度，a值越大，幅度越大（范围取0-45不等，幅度越大，左右转弯的限制就越大）
	     #return :void
	     #
        self.a = (int)(45 * percent / 100.0)
        pass

    def setSwingHz(self,percent):
        #name : set_w：修改频率
        #decription : w值改变时要执行的函数，它是通过改变公式里的参数w
        #arguments : the_w
        #即目标w值（范围取20-140不等，可适当缩小范围），w值决定一个运动波形周期内，关节刷新角度的密集程度，w越小，则刷新角度越密集，理论上走的越顺畅
        #return :void
        #
        self.w = 20 + (int)(40.0 * percent / 100.0)
        pass
